#include "navi_controller/controller_server.hpp"
#include <iostream>

namespace navi_controller
{

    ControllerServer::ControllerServer(const rclcpp::NodeOptions &options)
        : nav2_util::LifecycleNode("controller_server", "", options),
          controller_plugin_("navi_pure_pursuit_controller::PurePursuitController"),
          // 功能包name，控制器基类的name
          controller_loader_("navi_core", "navi_core::Controller")
    {
        RCLCPP_INFO(get_logger(), "Creating controller ");
        declare_parameter("controller_plugin", controller_plugin_);
    }
    ControllerServer::~ControllerServer()
    {
    }
    nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_configure");
        auto node = shared_from_this();
        get_parameter("controller_plugin", controller_plugin_);
        RCLCPP_INFO(get_logger(), "controller_plugin: %s", controller_plugin_.c_str());
        controller_ = controller_loader_.createUniqueInstance(controller_plugin_);//type
        controller_->configure(node, "PurePursuitController");
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn ControllerServer::on_activate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_activate");
        controller_->activate();
        createBond();
        controller_->setPlan();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn ControllerServer::on_deactivate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_deactivate");
        controller_->deactivate();
        destroyBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn ControllerServer::on_cleanup(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_cleanup");
        controller_->cleanup();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn ControllerServer::on_shutdown(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_shutdown");
        return nav2_util::CallbackReturn::SUCCESS;
    }

} // namespace navi_controller